﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Shapes;

using KinectAdapter;
using NXT2Adapter;
using System.Timers;
using Microsoft.Kinect;
using System.Windows.Threading;
using System.Threading;

namespace Kinixt
{
    /// <summary>
    /// Interaction logic for HUD.xaml
    /// </summary>
    public partial class HUD : Window
    {
        #region Fields and constants

        private const double ROBOT_CONTRL_INTERVAL = 50;
        private const double SKELETON_INTERVAL = 5000;

        private Menu menu;
        
        private KinectForkliftAdapter kinectAdapter = null;
        private RobotControl nxt2Adapter = null;
        private VideoPlayer videoPlayer;

        private System.Timers.Timer robotControlTimer = null;
        private System.Timers.Timer skeletonControlTimer = null;

        private DependencyPropertyChangedEventHandler kinectDependencyPropertyChangedEventHandler = null;
        private ElapsedEventHandler robotControlElapsedEventHandler = null;
        private ElapsedEventHandler skeletonElapsedEventHandler = null;

        #endregion

        #region Public methods

        /// <summary>
        /// Initializes the HUD window. Start() needs to be called for it to actually start working.
        /// </summary>
        /// <param name="menu">Menu window.</param>
        /// <param name="adapterName">Name of the specific KinectForkliftAdapter type that should be used.</param>
        /// <param name="controlName">Name of the specific RobotControl type that should be used.</param>
        /// <param name="transmitterIp">IP address of the Android device that will transmit video.</param>
        /// <param name="isPreviewOn">True if preview image from Kinect should be displayed.</param>
        public HUD(Menu menu, string adapterName, string controlName, string transmitterIp, bool isPreviewOn, byte comPort)
        {
            InitializeComponent();
            

            this.menu = menu;

            // creating adapters
            this.kinectAdapter = CreateKinectAdapter(adapterName);
            //this.nxt2Adapter = CreateRobotControl(controlName, comPort);

            // creating timers
            robotControlTimer = new System.Timers.Timer(ROBOT_CONTRL_INTERVAL);
            robotControlTimer.Enabled = true;
            skeletonControlTimer = new System.Timers.Timer(SKELETON_INTERVAL);
            skeletonControlTimer.Enabled = true;

            // handling events
            kinectDependencyPropertyChangedEventHandler = new DependencyPropertyChangedEventHandler(kinectSensorChooser1_KinectSensorChanged);
            kinectSensorChooser1.KinectSensorChanged += kinectDependencyPropertyChangedEventHandler;
            kinectAdapter.StopIssued += new EventHandler(kinectAdapter_StopIssued);
            kinectAdapter.MenuIssued += new EventHandler(kinectAdapter_MenuIssued);            
            robotControlElapsedEventHandler = new ElapsedEventHandler(robotControlTimer_Elapsed);
            skeletonElapsedEventHandler = new ElapsedEventHandler(skeletonTimer_Elapsed);

            // kinect preview
            if (!isPreviewOn)
            {
                hudCanvas.Children.Remove(kinectDepthViewer1);
                hudCanvas.Children.Remove(kinectSkeletonViewer1);
            }

            SetBindings();

            videoPlayer = new VideoPlayer(hudCanvas, transmitterIp);

            SwitchTo();
            this.nxt2Adapter = CreateRobotControl(controlName, comPort);
        }

        /// <summary>
        /// HUD connects all adapters, makes necessary connections and starts working.
        /// </summary>
        public void Start()
        {
            int notConnected = 0;

            if (nxt2Adapter.Connect() == false)
            {
                // TODO
                // connection failed; for example devices not paired
                notConnected++;
            }
            else
            {
                //if (nxt2Adapter is SimulationControl)
                //{
                //    hudGrid.Children.Add(((SimulationControl)nxt2Adapter).RobotSimulation);
                //}
                robotControlTimer.Elapsed += robotControlElapsedEventHandler;
            }

            if (kinectAdapter.Start() == false)
            {
                // TODO
                // kinect not started; for example not connected
                notConnected++;
            }
            else
            {
                skeletonControlTimer.Elapsed += skeletonElapsedEventHandler;
            }
            
            videoPlayer.Start();    // starting video
        }

        /// <summary>
        /// Releases all recources and closes all connections;
        /// </summary>
        public void Destroy()
        {
            StopNxt2Adapter();
            StopKinectAdapter();            
            StopKinect(kinectSensorChooser1.Kinect);

            videoPlayer.Stop(); // stopping video
        }

        /// <summary>
        /// Switches from menu to HUD window.
        /// </summary>
        public void SwitchTo()
        {
            App.Current.MainWindow = this;
            menu.Hide();
            this.Show();
            this.kinectAdapter.IsActive = true;
        }

        #endregion

        #region Private utility methods

        /// <summary>
        /// Creates a KinectForkliftAdapter object of type selected in the appropriate comboBox.
        /// </summary>
        /// <param name="adapterName">Name of the specific KinectForkliftAdapter type.</param>
        /// <returns>Initialized KinectForkliftAdapter object of desired type. Returns null if the name if unrecognized.</returns>
        private KinectForkliftAdapter CreateKinectAdapter(string adapterName)
        {
            switch (adapterName)
            {
                case KinectForkliftAdapter.ButlaStyle:
                    return new ButlaStyle();
                case KinectForkliftAdapter.AllInRightHand:
                    return new AllInRightHand();
                case KinectForkliftAdapter.SteeringWheel:
                    return new SteeringWheel();
                default:
                    return null;
            }
        }

        /// <summary>
        /// Creates a RobotControl object of type selected in the appropriate comboBox.
        /// </summary>      
        /// <param name="controlName">Name of the specific RobotControl type.</param>
        /// <returns>Initialized RobotControl object of desired type. Returns null if the name if unrecognized.</returns>
        private RobotControl CreateRobotControl(string controlName, byte comPort)
        {
            switch (controlName)
            {
                case RobotControl.Mindsqualls:
                    return new MindsquallsControl((byte)6);
                case RobotControl.Simulation:
                    return new SimulationControl(this.simulationCanvas);
                //this.mainGrid.Children.Add(((SimulationControl)nxt2Adapter).RobotSimulation);
                default:
                    return null;
            }
        }

        /// <summary>
        /// Sets bindings between HUD controls and adapters.
        /// </summary>
        private void SetBindings()
        {
            DataContext = kinectAdapter;
            BindingMode bindingMode = BindingMode.TwoWay;
            speedSlider.SetBinding(Slider.ValueProperty, new Binding("Z.Value") { Mode = bindingMode });
            turnSlider.SetBinding(Slider.ValueProperty, new Binding("X.Value") { Mode = bindingMode });
            forkSlider.SetBinding(Slider.ValueProperty, new Binding("Y.Value") { Mode = bindingMode });
            mainSteeringBox.SetBinding(CheckBox.IsCheckedProperty, new Binding("MainSteeringOn") { Mode = BindingMode.OneWay });
            secondarySteeringBox.SetBinding(CheckBox.IsCheckedProperty, new Binding("SecondarySteeringOn") { Mode = BindingMode.OneWay });
            //sensorUltraSonicLabel.SetBinding(Label.ContentProperty, new Binding("SensorUltraSonic") { Mode = BindingMode.OneWay, Source = nxt2Adapter });
        }

        void StopKinect(KinectSensor sensor)
        {
            if (sensor != null)
            {
                sensor.DepthStream.Disable();
                sensor.SkeletonStream.Disable();
                sensor.Stop();
                sensor.AudioSource.Stop();
            }
        }

        private void StopKinectAdapter()
        {
            if (kinectAdapter != null)
            {
                skeletonControlTimer.Elapsed -= skeletonElapsedEventHandler;
                kinectAdapter.Stop();
            }
        }

        private void StopNxt2Adapter()
        {
            if (nxt2Adapter != null)
            {
                robotControlTimer.Elapsed -= robotControlElapsedEventHandler;
                nxt2Adapter.Disconnect();

                if (nxt2Adapter.GetType() == typeof(SimulationControl))
                {
                    hudGrid.Children.Remove(((SimulationControl)nxt2Adapter).RobotSimulation);
                }               
            }
        }

        private void buildJointString(JointType jointType, Skeleton skeleton, StringBuilder sb)
        {
            sb.Append(jointType.ToString() + " : \n");
            sb.Append("X : " + skeleton.Joints[jointType].Position.X + "\n");
            sb.Append("Y : " + skeleton.Joints[jointType].Position.Y + "\n");
            sb.Append("Z : " + skeleton.Joints[jointType].Position.Z + "\n");
            sb.Append("\n");
        }

        private void SetTextToDebugTextbox(string text)
        {
            menu.debugTextBox.Text = text;
        }

        private void AddTextToDebugTextbox(string text)
        {
            menu.debugTextBox.Text += text;
        }

        #endregion

        #region Event handling

        /// <summary>
        /// Switches back to menu window.
        /// </summary>
        private void Window_Closed(object sender, EventArgs e)
        {
            menu.SwitchTo(true);
        }

        void kinectSensorChooser1_KinectSensorChanged(object sender, DependencyPropertyChangedEventArgs e)
        {
            KinectSensor oldSensor = (KinectSensor)e.OldValue;
            StopKinect(oldSensor);

            KinectSensor newSensor = (KinectSensor)e.NewValue;

            newSensor.DepthStream.Enable();
            newSensor.SkeletonStream.Enable();

            try
            {
                newSensor.Start();
            }
            catch (System.IO.IOException)
            {
                kinectSensorChooser1.AppConflictOccurred();
            }
        }

        private void robotControlTimer_Elapsed(object sender, ElapsedEventArgs e)
        {
            if (kinectAdapter.MainSteeringOn == true)
            {
                nxt2Adapter.Move(kinectAdapter.Z.Value, 0, kinectAdapter.X.Value);
            }
            else
            {
                //nxt2Adapter.IdleMove();
            }

            if (kinectAdapter.SecondarySteeringOn == true)
            {
                nxt2Adapter.ControlFork(kinectAdapter.Y.Value);
            }
            else
            {
                //nxt2Adapter.IdleFork();
            }
        }

        private void skeletonTimer_Elapsed(object sender, ElapsedEventArgs e)
        {
            StringBuilder sb = new StringBuilder();
            Skeleton skeleton = kinectAdapter.OperatorSkeleton.OperatingSkeleton;

            if (skeleton != null)
            {
                buildJointString(JointType.HandRight, skeleton, sb);
                buildJointString(JointType.ShoulderRight, skeleton, sb);

                buildJointString(JointType.HandLeft, skeleton, sb);
                buildJointString(JointType.ShoulderLeft, skeleton, sb);

                buildJointString(JointType.ShoulderCenter, skeleton, sb);

                buildJointString(JointType.Spine, skeleton, sb);
            }
            else
            {
                sb.Append("skeketon == NULL");
            }

            this.Dispatcher.Invoke(
                DispatcherPriority.Render,
                (SendOrPostCallback)delegate
                {
                    SetTextToDebugTextbox(sb.ToString());
                },
                null);
        }

        private void Window_Loaded(object sender, RoutedEventArgs e)
        {
            kinectSensorChooser1.KinectSensorChanged += kinectDependencyPropertyChangedEventHandler;
            kinectSensorChooser1.KinectSensorChanged += new DependencyPropertyChangedEventHandler(kinectSensorChooser1_KinectSensorChanged);
        }

        private void mainSteeringBox_Checked(object sender, RoutedEventArgs e)
        {
            if (kinectAdapter != null)
            {
                kinectAdapter.MainSteeringOn = mainSteeringBox.IsChecked.Value;
            }
        }

        private void secondarySteeringBox_Checked(object sender, RoutedEventArgs e)
        {
            if (kinectAdapter != null)
            {
                kinectAdapter.SecondarySteeringOn = secondarySteeringBox.IsChecked.Value;
            }
        }

        /// <summary>
        /// Returns to menu window and stops the forklift.
        /// </summary>
        void kinectAdapter_MenuIssued(object sender, EventArgs e)
        {
            kinectAdapter.IsActive = false;
            kinectAdapter.Halt();
            menu.SwitchTo(false);
        }

        /// <summary>
        /// Stops the forklift.
        /// </summary>
        void kinectAdapter_StopIssued(object sender, EventArgs e)
        {
            kinectAdapter.Halt();
        }

        /// <summary>
        /// DEBUG
        /// </summary>
        private void sensorLightBox_Checked(object sender, RoutedEventArgs e)
        {
            //nxt2Adapter.NxtLightSensorGenerateLight = (bool)sensorLightBox.IsChecked;
        }

        /// <summary>
        /// DEBUG
        /// </summary>
        private void button1_Click(object sender, RoutedEventArgs e)
        {
            menu.SwitchTo(false);
        }

        #endregion       
    }
}
